﻿using System;
using System.Collections.Generic;
using System.Text;
using ASquared;

namespace ASquared.ModelOptimization
{
    public enum ConstraintType { Equality, LeftLessThanRight, LeftGreaterThanRight }
    public enum VariableType { Free, NonNegative, Negative }

    public class LinearSolver
    {
        public static Boolean throwExceptionOnNoSolution = true;
        public static Boolean throwExceptionOnUnboundedSolution = false; 
        private Matrix _A, _b, _c;
        private Matrix _cT, _cNT, _cBT, _cRed;
        private Matrix _AOrig, _bOrig, _cOrig;
        private Matrix _x, _xB;
        private Matrix _tableau, _APhaseII, _N, _Binv, _yT;
        private Double _z;
        private List<Int32> _Bcols, _Ncols;
        private bool _PhaseI, _solved, _noFeasSol, _unBounded, _nonUniqueSol;
        private Int32[] _negCounterpart;
        private VariableType[] _vTypes;
        private ConstraintType[] _cTypes;

        #region Properties

        /// <summary>Gets the matrix of minimizers</summary>
        public Matrix x
        {
            get { return _x; }
        }
        /// <summary>Gets the constraint coefficient matrix</summary>
        public Matrix A
        {
            get { return _A; }
        }
        /// <summary>Gets the right-hand side vector</summary>
        public Matrix b
        {
            get { return _b; }
        }
        /// <summary>Gets the costs vector</summary>
        public Matrix c
        {
            get { return _c; }
        }
        /// <summary>Gets the optimum value</summary>
        public Double Optimum
        {
            get { return _z; }
        }
        /// <summary>Gets whether a solution was found</summary>
        public bool SolutionFound
        {
            get { return _solved; }
        }
        /// <summary>Gets a value specifying if there is no feasible solution</summary>
        public bool NoFeasibleSolution
        {
            get { return _noFeasSol; }
        }
        /// <summary>Gets whether the solution is unbounded</summary>
        public bool UnBounded
        {
            get { return _unBounded; }
        }
        /// <summary>Gets whether there is a non-unique solution</summary>
        public bool NonUniqueSol
        {
            get { return _nonUniqueSol; }
        }
        /// <summary>Gets and sets an array specifying the variable types.</summary>
        public VariableType[] VariableTypes
        {
            get { return _vTypes; }
            set
            {
                if (value.Length == _A.NumOfCols)
                    _vTypes = value;
                else
                    throw new Exception("The number of specified variable types needs to be the same as the number of columns in 'A' and 'c'.");
            }
        }
        /// <summary>Gets and sets an array specifying the constraint types.</summary>
        public ConstraintType[] ConstraintTypes
        {
            get { return _cTypes; }
            set
            {
                if (value.Length == _A.NumOfRows)
                    _cTypes = value;
                else
                    throw new Exception("The number of specified constraint types needs to be the same as the number of rows in 'A' and 'b'.");
            }
        }

        #endregion

        #region Constructor

        /// <summary>Constructs a linear solver instance from a problem in standard form.</summary>
        /// <param name="A">The constraint coefficient matrix</param>
        /// <param name="b">The "right-hand side" of the constraints</param>
        /// <param name="c">The costs of the decisions in the objective function</param>
        public LinearSolver(Matrix A, Matrix b, Matrix c)
        {
            _A = A.Copy();
            _b = (b.NumOfRows == 1) ? b.Transpose() : b.Copy();
            _c = (c.NumOfRows == 1) ? c.Transpose() : c.Copy();
        }

        #endregion

        #region Build problem methods

        /// <summary>Initializes </summary>
        private void InitializeTypes()
        {
            if (_vTypes == null)
            {
                _vTypes = new VariableType[_A.NumOfCols];
                for (Int32 i = 0; i < _A.NumOfCols; i++)
                    _vTypes[i] = VariableType.NonNegative;
            }
            _negCounterpart = new Int32[_A.NumOfCols];
            if (_cTypes == null)
            {
                _cTypes = new ConstraintType[_A.NumOfRows];
                for (Int32 i = 0; i < _A.NumOfRows; i++)
                    _cTypes[i] = ConstraintType.Equality;
            }
        }
        /// <summary>Defines variable types for a given set of variables within the constraint coefficient matrix. The rest are assumed to be non-negative.</summary>
        private void DefineVariableTypes()
        {
            for (Int32 i = 0; i < _vTypes.Length; i++)
            {
                // Add appropriate variable types
                switch (_vTypes[i])
                {
                    case VariableType.NonNegative:
                        break; // do nothing
                    case VariableType.Negative:
                        // Replace the variable in A with a negative variable
                        for (Int32 j = 0; j < _A.NumOfRows; j++)
                            _A[j, i] = -_A[j, i];
                        _c[i] = -_c[i];
                        break;
                    case VariableType.Free:
                        // Add a negative variable to the end
                        _A.AppendCols(1);
                        for (Int32 j = 0; j < _A.NumOfRows; j++)
                            if (_A[j, i] != 0)
                                _A[j, _A.NumOfCols - 1] = -_A[j, i];
                        _c.AppendRows(1);
                        _c[_c.NumOfRows - 1] = -_c[i];
                        _negCounterpart[i] = _c.NumOfRows - 1;
                        break;
                    default:
                        throw new NotImplementedException("variable type " + _vTypes[i].ToString() + " not implemented.");
                }
            }
        }
        /// <summary>Adds a constraint to the linear solver.</summary>
        private void DefineConstraints()
        {
            for (Int32 i = 0; i < _cTypes.Length; i++)
            {
                switch (_cTypes[i])
                {
                    case ConstraintType.Equality:
                        break; // do nothing
                    case ConstraintType.LeftGreaterThanRight:
                        // Add appropriate surplus variable 
                        _A.AppendCols(1);
                        _A[i, _A.NumOfCols - 1] = -1;
                        _c.AppendRows(1);
                        break;
                    case ConstraintType.LeftLessThanRight:
                        // Add appropriate slack variable
                        _A.AppendCols(1);
                        _A[i, _A.NumOfCols - 1] = 1;
                        _c.AppendRows(1);
                        break;
                    default:
                        throw new NotImplementedException("constraint type " + _cTypes[i].ToString() + " not implemented.");
                }
                if (_b[i] < 0)
                {
                    _b[i] = -_b[i];
                    _A.ReplaceRow(i, -1 * A.GetRow(i));
                }
            }
        }

        #endregion

        #region Solver methods

        private void Checks()
        {
            if (_A == null || _b == null || _c == null)
                throw new NullReferenceException("One of the required matrices is null. Check inputs.");
            if (_A.NumOfRows != _b.NumOfRows)
                throw new Exception("'A' must have the same number of rows as elements in 'b'.");
            if (_A.NumOfCols != _c.NumOfRows)
                throw new Exception("'A' must have the same number of columns as elements in 'c'.");

        }
        private void BuildTableau()
        {
            if (_PhaseI)
            {
                // add columns to A 
                Matrix newcols = Matrix.Identity(_A.NumOfRows);
                _A.AppendCols(newcols);
                newcols.AppendCols(_b);

                // fill cT 
                Matrix newcosts = new Matrix(1, _A.NumOfRows, 1.0);
                _cT = new Matrix(1, _c.NumOfRows, 0.0);
                _cT.AppendCols(newcosts);

                // fill tableau
                Matrix zeroCosts = new Matrix(1, _A.NumOfRows, 0.0);
                Matrix firstrow = zeroCosts.Copy();
                firstrow.AppendCols(new Matrix((Int32)1, (Int32)1, -(newcosts * _b)[0]));
                newcols.InsertRowsAt(firstrow, 0);
                _tableau = newcols;

            }
            else
            {
                Matrix newrow = new Matrix(1, _tableau.NumOfCols, 0.0);
                Matrix I = Matrix.Identity(_tableau.NumOfRows - 1);
                _cT = _c.Transpose();
                newrow[newrow.NumOfCols - 1] = -(_cT * _x)[0];
                _tableau.ReplaceRow(0, newrow);
                for (Int32 i = 1; i < _tableau.NumOfRows; i++)
                    for (Int32 j = 0; j < _tableau.NumOfCols - 1; j++)
                        _tableau[i, j] = I[i - 1, j];
                _b = _Binv * _b;
                _A = _Binv * _APhaseII;
            }
        }
        private void Initialize()
        {
            if (_PhaseI)
            {
                _xB = _b;

                // columns in the basis
                _Bcols = new List<Int32>(_APhaseII.NumOfRows);
                for (Int32 i = 0; i < _APhaseII.NumOfRows; i++)
                    _Bcols.Add(_APhaseII.NumOfCols + i);

                // columns not in the basis 
                _Ncols = new List<Int32>(_APhaseII.NumOfCols);
                for (Int32 i = 0; i < _APhaseII.NumOfCols; i++)
                    _Ncols.Add(i);
            }
            else
            {
                for (Int32 i = _A.NumOfCols; i < _A.NumOfRows + _A.NumOfCols; i++)
                {
                    if (_Bcols.Contains(i))
                        _noFeasSol = true;
                    if (_Ncols.Contains(i))
                        _Ncols.Remove(i);
                }
            }
        }
        private void UpdateBasis()
        {
            _Binv = _tableau.Copy();
            _Binv.RemoveRowAt(0);
            _Binv.RemoveColAt(_tableau.NumOfCols - 1);
            _cBT = _cT.GetCols(_Bcols.ToArray());
            _xB = _Binv * _b;
            _yT = _cBT * _Binv;
        }
        private void UpdateNonBasis()
        {
            _N = _A.GetCols(_Ncols.ToArray());
            _cNT = _cT.GetCols(_Ncols.ToArray());
            _cRed = _cNT - _yT * _N;
        }
        private Int32 RatioTest(Matrix A_j)
        {
            Double currVal, minVal = Double.MaxValue;
            Int32 min_i = -1;
            for (Int32 i = 0; i < A_j.NumOfRows; i++)
                if (A_j[i] > 0 && (currVal = _b[i] / A_j[i]) < minVal)
                {
                    minVal = currVal;
                    min_i = i;
                }
            return min_i;
        }
        private void SwitchingVariables()
        {
            // Pricing out
            Double minVal;
            Int32 row, col, enteringVarIndex;
            minVal = _cRed.Min(out row, out enteringVarIndex);
            Int32 enteringVar = _Ncols[enteringVarIndex];
            Matrix A_j = _Binv * _A.GetCol(enteringVar);

            // Ratio test 
            Matrix ratios = _b.Copy();
            for (Int32 i = 0; i < ratios.NumOfRows; i++)
                ratios[i] = (A_j[i] > 0) ? ratios[i] / A_j[i] : Double.MaxValue;
            Int32 leavingVarIndex;
            Int32[] rowI, colI;
            Double minRatio = ratios.Min(out leavingVarIndex, out col, out rowI, out colI);

            // Break ties
            if (minRatio != Double.MaxValue)
            {
                List<Int32> rowIs = new List<Int32>(rowI);
                for (Int32 j = 0; rowIs.Count > 1 && j < _A.NumOfCols; j++)
                {
                    for (Int32 i = 0; i < ratios.Length; i++)
                        ratios[i] = rowIs.Contains(i) ? _A[i, j] / A_j[i] : Double.MaxValue;
                    ratios.Min(out leavingVarIndex, out col, out rowI, out colI);
                    rowIs = new List<Int32>(rowI); 
                }
                if (rowIs.Count > 1)
                    throw new Exception("There is a redundant constraint within the LP setup.");
            } 

            Int32 leavingVar = _Bcols[leavingVarIndex];
            if (minVal < 0)
            {
                // Switch the variables (a basic for a non-basic variable)
                Matrix redC = new Double[] { minVal };
                redC.AppendRows(A_j);
                _tableau.InsertColsAt(redC, 0);
                if (minRatio == Double.MaxValue)
                {
                    _unBounded = true;
                }
                else
                {
                    _tableau.Pivot(leavingVarIndex + 1, 0);
                    _tableau.RemoveColAt(0);
                    _Bcols.RemoveAt(leavingVarIndex);
                    _Bcols.Insert(leavingVarIndex, enteringVar);
                    _Ncols.RemoveAt(enteringVarIndex);
                    _Ncols.Insert(enteringVarIndex, leavingVar);
                }
            }
            else if (minVal == 0)
            {
                if (!_PhaseI)
                {
                    // We have a solution, but it's nonunique 
                    if (minRatio == Double.MaxValue) _unBounded = true;
                    _nonUniqueSol = true;
                }
                _solved = true; // non-unique solutions exist
            }
            else
            {
                _solved = true; // Optimimum found! 
            }
        }
        private void GetOutputFromTableau()
        {
            Matrix output = _tableau.GetCol(_tableau.NumOfCols - 1);
            _xB = output.GetRows(1, output.NumOfRows - 1);
            if (_PhaseI)
            {
                _x = new Double[_APhaseII.NumOfCols];
                for (Int32 i = 0; i < _Bcols.Count; i++)
                    _x[_Bcols[i]] = _xB[i];
            }
            else
            {
                _x = new Double[_AOrig.NumOfCols];
                for (Int32 i = 0; i < _x.Length; i++)
                {
                    Int32 ind = _Bcols.IndexOf(i);
                    switch (_vTypes[i])
                    {
                        case VariableType.NonNegative:
                            if (ind != -1)
                                _x[i] = _xB[ind];
                            break;
                        case VariableType.Negative:
                            if (ind != -1)
                                _x[i] = -_xB[ind];
                            break;
                        case VariableType.Free:
                            if (ind != -1)
                                _x[i] = _xB[ind];
                            ind = _Bcols.IndexOf(_negCounterpart[i]);
                            if (ind != -1)
                                _x[i] = -_xB[ind];
                            break;
                        default:
                            throw new NotImplementedException("variable type " + _vTypes[i].ToString() + " not implemented.");
                    }
                }
            }
            _z = -output.GetRows(0, 0)[0];
        }
        private void SolveLP()
        {
            this.BuildTableau();
            this.Initialize();
            while (!_solved && !_unBounded && !_noFeasSol)
            {
                this.UpdateBasis();
                this.UpdateNonBasis();
                this.SwitchingVariables();
            }
            if (throwExceptionOnNoSolution && _noFeasSol)
                    throw new Exception("No feasible solution could be found for the LP problem.");
            if (throwExceptionOnUnboundedSolution && _unBounded)
                    throw new Exception("The solution to the LP problem is unbounded.");
            this.UpdateBasis();
            this.UpdateNonBasis();
            this.GetOutputFromTableau();
        }
        private void PhaseI()
        {
            _solved = false;
            _noFeasSol = false;
            _nonUniqueSol = false;
            _unBounded = false;
            _PhaseI = true;
            this.SolveLP();
        }
        private void PhaseII()
        {
            _solved = false;
            _noFeasSol = false;
            _nonUniqueSol = false;
            _unBounded = false;
            _PhaseI = false;
            this.SolveLP();
        }

        public void Solve()
        {
            // Perform checks
            this.Checks();

            // Save originals 
            _AOrig = _A.Copy();
            _bOrig = _b.Copy();
            _cOrig = _c.Copy();

            // Define variable types and constraint types
            this.InitializeTypes();
            this.DefineVariableTypes();
            this.DefineConstraints();

            // Save the originals
            _APhaseII = _A.Copy();

            // Solve phase I for feasibility, phase II for solution
            PhaseI();
            PhaseII();

            // Restore originals back to originals
            _A = _AOrig;
            _b = _bOrig;
            _c = _cOrig;
        }

        #endregion

        #region Overrides

        public override string ToString()
        {
            StringBuilder s = new StringBuilder("Solve: z = ");
            for (Int32 i = 0; i < _c.NumOfRows; i++)
            {
                if (_c[i] != 0)
                {
                    s.Append((_c[i] == 1 ? "" : _c[i].ToString()) + "x_" + (i + 1).ToString());
                    if (i != _c.NumOfRows - 1) s.Append(" + ");
                }
            }
            s.Append("\nConstraints:\n");
            for (Int32 i = 0; i < _A.NumOfRows; i++)
            {
                for (Int32 j = 0; j < _A.NumOfCols; j++)
                {
                    if (_A[i, j] != 0)
                    {
                        s.Append((_A[i, j] == 1 ? "" : _A[i, j].ToString()) + "x_" + (j + 1).ToString());
                        if (j != _A.NumOfCols - 1) s.Append(" + ");
                    }
                }
                s.Append(" = " + _b[i].ToString() + "\n");
            }
            return s.ToString();
        }

        #endregion
    }
}
